import robomaster
from robomaster import robot
import time

def sub_battery_info_handler(batter_info, ep_robot):
    percent = batter_info
    # print("Battery: {0}%.".format(percent))
    ep_led = ep_robot.led
    brightness = int(percent * 255 / 100)
    ep_led.set_led(comp="all", r=brightness, g=brightness, b=brightness)
    
    if (percent < 30):
        print("Battery: {0}%.".format(percent))
        print("请充电！！请充电！！请充电！！")

def sub_sensor_info_handler(sub_info):
    distance = sub_info
    print("tof1:{0}  tof2:{1}  tof3:{2}  tof4:{3}".format(distance[0], distance[1], distance[2], distance[3]))

if __name__ == '__main__':
    ep_robot = robot.Robot()
    ep_robot.initialize(conn_type="ap")
    print("===========connect success!===========")

    ep_version = ep_robot.get_version()
    print("Robot Version: {0}".format(ep_version))

    ep_battery = ep_robot.battery

    ep_battery.sub_battery_info(5, sub_battery_info_handler, ep_robot)
    time.sleep(1)
    ep_battery.unsub_battery_info()

    ep_sensor = ep_robot.sensor
    ep_sensor.sub_distance(freq=5, callback=sub_sensor_info_handler)
    time.sleep(5)
    ep_sensor.unsub_distance()

    ep_robot.close()